#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Sensor, S2,     GYRO,                sensorI2CHiTechnicGyro)
#pragma config(Motor,  mtr_S1_C1_1,     leftmotors,    tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightmotors,   tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     belt,          tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     door,          tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     launch,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Autonomous Mode Code Template
//
// This file contains a template for simplified creation of an autonomous program for an TETRIX robot
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "drivers/HTGYRO-driver.h"

float currHeading = 0;

task getHeading () {
	float delTime = 0;
	float prevHeading = 0;
	float curRate = 0;

  HTGYROstartCal(GYRO);
  PlaySound(soundBeepBeep);
  while (true) {
    time1[T1] = 0;
    curRate = HTGYROreadRot(GYRO);
    if (abs(curRate) > 3) {
      prevHeading = currHeading;
      currHeading = prevHeading + curRate * delTime;
      if (currHeading > 360) currHeading -= 360;
      else if (currHeading < 0) currHeading += 360;
    }
    wait1Msec(5);
    delTime = ((float)time1[T1]) / 1000;
    //delTime /= 1000;
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of autonomous mode, you may want to perform some initialization on your robot.
// Things that might be performed during initialization include:
//   1. Move motors and servos to a preset position.
//   2. Some sensor types take a short while to reach stable values during which time it is best that
//      robot is not moving. For example, gyro sensor needs a few seconds to obtain the background
//      "bias" value.
//
// In many cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
//   1. Have the robot follow a line on the game field until it reaches one of the puck storage
//      areas.
//   2. Load pucks into the robot from the storage bin.
//   3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

task main()
{
  initializeRobot();

  waitForStart(); // Wait for the beginning of autonomous phase.

StartTask(getHeading);

  int x;
nMotorEncoder[leftmotors] = 0;
nMotorEncoder[rightmotors] = 0;


nMotorEncoderTarget[leftmotors] = 4320;
nMotorEncoderTarget[rightmotors] = 4320;
  motor[leftmotors] = 85;
  motor[rightmotors] = 85;
  PlaySound(soundBeepBeep);
  wait1Msec(3000);

  while(nMotorRunState[leftmotors] != runStateIdle || nMotorRunState[rightmotors] != runStateIdle)
  {

  }

  while(x < 10)
  {
    if(currHeading < 90)
    {
     motor[leftmotors] = 85;
     motor[rightmotors] = -85;

     wait1Msec(200);
     x = 2;
    }
    if(currHeading > 90)
    {
     motor[leftmotors] = 0;
     motor[rightmotors] = 0;
     wait1Msec(200);
     x = 13;
    }
  }

   while(10 << x << 20)
  {
    nMotorEncoder[leftmotors] = 0;
    nMotorEncoder[rightmotors] = 0;

    nMotorEncoderTarget[leftmotors] = 2880;
    nMotorEncoderTarget[rightmotors] = 2880;
     motor[leftmotors] = 85;
     motor[rightmotors] = 85;
     PlaySound(soundDownwardTones);
     wait1Msec(2500);

     while(nMotorRunState[leftmotors] != runStateIdle || nMotorRunState[rightmotors] != runStateIdle)
  {

  }

    if(currHeading < 113)
    {
     motor[leftmotors] = 85;
     motor[rightmotors] = -85;
     wait1Msec(200);
     x = 15;
    }
    if(currHeading > 113)
    {
     motor[leftmotors] = 0;
     motor[rightmotors] = 0;
     wait1Msec(200);
     x = 22;
    }
  }

    while(20 << x << 30)
   {

   nMotorEncoder[leftmotors] = 0;
    nMotorEncoder[rightmotors] = 0;

    nMotorEncoderTarget[leftmotors] = 5760;
    nMotorEncoderTarget[rightmotors] = 5760;
     motor[leftmotors] = 85;
     motor[rightmotors] = 85;
     PlaySound(soundUpwardTones);
     wait1Msec(8000);

    while(nMotorRunState[leftmotors] != runStateIdle || nMotorRunState[rightmotors] != runStateIdle)
   {

   }
     //possibly add sonar for bowling ball.

  }

}
